#include <tbb/parallel_for.h>
#include <tbb/tbb.h>
#include <tbb/mutex.h>
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <imgDefine/filesystem.h>
#include <basicOperate/foldersOperations.h>
#include <Matlab2c/matlabc.h>
#include <chrono>
#include "Matlab2c/matlabc.h"
#include <tbb/parallel_invoke.h>

using namespace cv;


int main()
{

    auto start = std::chrono::high_resolution_clock::now();

    std::string imgpath = "C:/data/photo/1.bmp";
////    std::vector <std::string> imgdir;
////    imgpro::IgFolder::TraverseImages(imgpath,imgdir);
    cv::Mat im = cv::imread(imgpath,cv::IMREAD_GRAYSCALE);
    std::cout << "im_size: " << im.size() << std::endl;
    std::cout << "im_rows: " << im.rows << std::endl;
    std::cout << "im_cols: " << im.cols << std::endl;
    uchar im_pixel_value = im.at<uchar>(4607,5327);

    std::cout << "im_grey_value: " << im_pixel_value << std::endl;

    //粗定位
    LocCircleResult result =  CoarseLocation(im);
    std::cout << "Center: (" << result.center_x << ", " << result.center_y << ")" << std::endl;
    std::cout << "Circle Radius: " << result.circle_rad << std::endl;
    std::cout << "Ring Outer Radius: " << result.ring_ex_rad << std::endl;
    std::cout << "Ring Inner Radius: " << result.ring_in_rad << std::endl;
    //生成圆模板
    cv::Mat circleTemplate = CreateCircleTemplate(result.circle_rad,0);

//    for (int r = 0; r < circleTemplate.rows; r++)
//    {
//        const float * ptr = circleTemplate.ptr<float>(r);
//        for (int c = 0; c < circleTemplate.cols; ++c) {
//            cout << ptr[c] << " ";
//        }
//        std::cout << endl;
//    }

    //生成圆环模板
    cv::Mat ringTemplate = CreateRingTemplate(result.ring_ex_rad,result.ring_in_rad,0);

    //裁剪前反转
    cv::bitwise_not(im,im);
    //裁剪
//    cv::Mat im_circle = CropCircleImage(im,result,0);

//    cv::Mat im_ring = CropRingImage(im,result,0);

    cv::Mat im_circle,im_ring;
    tbb::parallel_invoke(
        [&]() {im_circle = CropCircleImage(im,result,0);},
        [&]() {im_ring = CropRingImage(im,result,0);}
        );


//    std::cout << "im_circle.size: " << im_circle.size() << std::endl;
//    std::cout << "im_ring.size: " << im_ring.size() << std::endl;

    //提取有效数据点
    std::vector<cv::Point3f> points_circle,points_ring;
//    std::vector<float> values_circle,values_ring;

//    cv::namedWindow("Im_ring window",cv::WINDOW_AUTOSIZE);
//    cv::imshow("Im_ring window",im_ring);
//    cv::waitKey(0);

   // ExtractNZero(im_circle,result.center_x,result.center_y,result.circle_rad,0,points_circle);
//    std::cout << "points_circle.size: " << points_circle.size() << std::endl;
//    std::cout << "points_circle0_x: " << points_circle[1].x << std::endl;
//    std::cout << "points_circle0_y: " << points_circle[1].y << std::endl;
//    std::cout << "points_circle0_z: " << points_circle[1].z << std::endl;
//    std::cout << "points_ring.size: " << points_ring.size() << std::endl;
   // ExtractNZero(im_ring,result.center_x,result.center_y,result.ring_ex_rad,0,points_ring);
    //std::cout << "points_ring.size: " << points_ring.size() << std::endl;

    tbb::parallel_invoke(
        [&]() {ExtractNZero(im_circle,result.center_x,result.center_y,result.circle_rad,0,points_circle);},
        [&]() {ExtractNZero(im_ring,result.center_x,result.center_y,result.ring_ex_rad,0,points_ring);}
        );


////    //拟合平面
    //Coeff coeff_circle =  PlaneSolu(points_circle);
//    std::cout << "a b c circle: " << coeff_circle.a0 << " " << coeff_circle.a1 << " " << coeff_circle.a2 << std::endl;
//    std::cout << "gray_value: " << points_circle[0].z << std::endl;

//    for (size_t i = 0; i < 10000; ++i)
//    {
//        std::cout << points_circle[i].x << ", " << points_circle[i].y << ", " << points_circle[i].z << std::endl;
//    }

    //Coeff coeff_ring = PlaneSolu(points_ring);
    //std::cout << "a b c ring: " << coeff_ring.a0 << " " << coeff_ring.a1 << " " << coeff_ring.a2 << std::endl;

//    for (size_t i = 0; i < 10000; ++i)
//    {
//        std::cout << points_ring[i].x << ", " << points_ring[i].y << ", " << points_ring[i].z << std::endl;
//    }

    Coeff coeff_circle,coeff_ring;
    tbb::parallel_invoke(
        [&]() {coeff_circle = PlaneSolu(points_circle);},
        [&]() {coeff_ring = PlaneSolu(points_ring);}
        );
//        std::cout << "a b c circle: " << coeff_circle.a0 << " " << coeff_circle.a1 << " " << coeff_circle.a2 << std::endl;
//        std::cout << "a b c ring: " << coeff_ring.a0 << " " << coeff_ring.a1 << " " << coeff_ring.a2 << std::endl;


//    double min_value,max_value;
//    cv::minMaxLoc(im_circle,&min_value,&max_value);
//    std::cout << "im_circle min value: " << min_value << std::endl;
//    std::cout << "im_circle max value: " << max_value << std::endl;

    cv::Mat im_circle_lo = cv::Mat::zeros(im_circle.size(),CV_8U);
    cv::Point center_circle(im_circle.cols / 2,im_circle.rows / 2);
    cv::circle(im_circle_lo,center_circle,result.circle_rad,cv::Scalar(255),-1);
    cv::Mat corrected_circle = CorrectGray(im_circle,coeff_circle,im_circle_lo,result.center_x,result.center_y,result.circle_rad,0);

    cv::Mat im_ring_lo = cv::Mat::zeros(im_ring.size(),CV_8U);
    cv::Point center_ring(im_ring.cols / 2,im_ring.rows / 2);
    cv::circle(im_ring_lo,center_ring,result.ring_ex_rad,cv::Scalar(255),-1);
    cv::circle(im_ring_lo,center_ring,result.ring_in_rad,cv::Scalar(0),-1);
    cv::Mat corrected_ring = CorrectGray(im_ring,coeff_circle,im_ring_lo,result.center_x,result.center_y,result.circle_rad,0);

//    cv::Mat result_circle,result_ring;
//    tbb::parallel_invoke(
//        [&](){result_circle = CorrectGray(im_circle,coeff_circle,im_circle_lo);},
//        [&](){result_ring = CorrectGray(im_ring,coeff_circle,im_ring_lo);}
//        );


    //std::cout << "corrected_circle.size(): " << corrected_circle.size() << std::endl;


    //std::cout << "result_ring.size(): " << corrected_ring.size() << std::endl;

//    for (int r = 0; r < 902; ++r)
//    {
//        const float *ptr = corrected_circle.ptr<float>(r);
//        for (int c = 0; c < 902; ++c)
//        {
//            std::cout << ptr[c] << ",";
//        }
//        std::cout << std::endl;
//    }

//    for (int r = 0; r < 902; ++r)
//    {
//        const float *ptr = corrected_ring.ptr<float>(r);
//        for (int c = 0; c < 902; ++c)
//        {
//            std::cout << ptr[c] << ",";
//        }
//        std::cout << std::endl;
//    }



    //调试 校正后的图像出现大额数值的问题
//    cv::Mat xx_circle = cv::Mat::zeros(im_circle.size(),CV_32F);
//    cv::Mat yy_circle = cv::Mat::zeros(im_circle.size(),CV_32F);
//    cv::Mat plane = cv::Mat::zeros(im_circle.size(),CV_32F);
//    int x_start = max(0, result.center_x - result.circle_rad - 0);
//    std::cout << "x_start: " << x_start << std::endl;

//    //int x_end = min(image.cols - 1, L.center_x + L.circle_rad + cut_wid);
//    int y_start = max(0, result.center_y - result.circle_rad - 0);
//    std::cout << "y_start: " << y_start << std::endl;


//    //填充X Y
//    for (int y = 0; y < xx_circle.rows; ++y)
//    {
//        for (int x = 0; x < yy_circle.cols; ++x)
//        {
//            xx_circle.at<float>(y,x) = x + x_start;
//            //std::cout << "xx_circle.at<" << y << "," << x << ">: " << xx_circle.at<float>(y,x) << std::endl;
//            //yy_circle.at<float>(x,y) = y + y_start;
//        }
//    }
//    xx_circle.convertTo(xx_circle,CV_32F);
//    //plane = static_cast<float>( coeff_circle.a0) * xx_circle;
//    plane = coeff_circle.a0 * xx_circle;
////    for (int r = 0; r < plane.rows; ++r)
////    {
////        for (int c = 0; c < plane.cols; ++c)
////        {
////            std::cout << plane.at<float>(r,c) << " ";
////        }
////        std::cout << std::endl;
////    }

//    for (int r = 0; r < plane.rows; ++r)
//    {
//        const float* ptr = plane.ptr<float>(r);
//        for (int c = 0; c < plane.cols; ++c)
//        {
//            std::cout << ptr[c] << " ";
//        }
//        std::cout << endl;
//    }

    //圆位置
    cv::Mat circle_flipped,ring_flipped;
    cv::flip(circleTemplate,circle_flipped,-1);
    cv::flip(ringTemplate,ring_flipped,-1);
    //std::cout << "circle_flipped: " << circle_flipped << std::endl;
    std::cout << "circle_flipped_size: " << circle_flipped.size() << std::endl;

//    int pad_x = circleTemplate.cols - 1;
//    int pad_y = circleTemplate.rows - 1;
//    cv::Mat padded_src;
//    int top = pad_y / 2,bottom = pad_y - top;
//    int left = pad_x / 2,right = pad_x - left;
//    cv::copyMakeBorder(circleTemplate,padded_src,top,bottom,left,right,cv::BORDER_CONSTANT,cv::Scalar(0));
//    cv::Mat res_circle;
//    cv::filter2D(padded_src,res_circle,-1,circle_flipped,cv::Point(-1,-1),0,cv::BORDER_CONSTANT);
//    std::cout << "res_circle: " << res_circle.size() << std::endl;


    tbb::parallel_invoke(
        [&]() { cv::filter2D(im_circle,im_circle,CV_32F,circle_flipped,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);},
        [&]() { cv::filter2D(im_ring,im_ring,CV_32F,ring_flipped,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);}
        );

//    cv::filter2D(im_circle,im_circle,CV_32F,circle_flipped,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);
//    cv::filter2D(im_ring,im_ring,CV_32F,ring_flipped,cv::Point(-1,-1),0,cv::BORDER_DEFAULT);


    tbb::parallel_invoke(
        [&]() { cv::normalize(im_circle,im_circle,0,1,NORM_MINMAX);},
        [&]() { cv::normalize(im_ring,im_ring,0,1,NORM_MINMAX);}
        );

//    cv::normalize(im_circle,im_circle,0,1,NORM_MINMAX);
//    cv::normalize(im_ring,im_ring,0,1,NORM_MINMAX);
    std::cout << "im_circle: " << im_circle.size() << std::endl;
    std::cout << "im_ring: " << im_ring.size() << std::endl;

//    for (int r = 0; r < im_circle.rows; r++)
//    {
//        const float * ptr = im_circle.ptr<float>(r);
//        for (int c = 0; c < im_circle.cols; c++)
//        {
//            std::cout << ptr[c] << ",";
//        }
//        std::cout << std::endl;
//    }

    //插值拟合 插值拟合后的mark点
      std::vector<float> x_pos_circle,y_pos_circle,x_pos_ring,y_pos_ring;

    tbb::parallel_invoke(
        [&]() { ProcessInterp(im_circle,x_pos_circle,y_pos_circle,result.center_x,result.center_y,result.circle_rad,0);},
        [&]() { ProcessInterp(im_ring,x_pos_ring,y_pos_ring,result.center_x,result.center_y,result.ring_ex_rad,0);}
        );

//        ProcessInterp(im_circle,x_pos_circle,y_pos_circle,result.center_x,result.center_y,result.circle_rad,0);
//        ProcessInterp(im_ring,x_pos_ring,y_pos_ring,result.center_x,result.center_y,result.ring_ex_rad,0);

//    std::cout << "x_pos_circle.size: " << x_pos_circle.size() << std::endl;
//    std::cout << "y_pos_circle.size: " << y_pos_circle.size() << std::endl;

//    for (auto pos_x : x_pos_circle)
//    {
//        std::cout << "circle_pos_x: " << pos_x << std::endl;
//    }

//    for (auto pos_y : y_pos_circle)
//    {
//        std::cout << "circle_pos_y: " << pos_y << std::endl;
//    }


//    for (auto pos_x : x_pos_ring)
//    {
//        std::cout << "ring_pos_x: " << pos_x << std::endl;
//    }

//    for (auto pos_y : y_pos_ring)
//    {
//        std::cout << "ring_pos_y: " << pos_y << std::endl;
//    }

    double x_pot_circle = Mean(x_pos_circle);
    double y_pot_circle = Mean(y_pos_circle);
    double x_pot_ring = Mean(x_pos_ring);
    double y_pot_ring = Mean(y_pos_ring);
    double x_error = x_pot_ring - x_pot_circle;
    double y_error = y_pot_ring - y_pot_circle;

    std::cout << "x_pot_circle: " << x_pot_circle << std::endl;
    std::cout << "y_pot_circle: " << y_pot_circle << std::endl;
    std::cout << "x_pot_ring: " << x_pot_ring << std::endl;
    std::cout << "y_pot_ring: " << y_pot_ring << std::endl;


    std::cout << "x_error: " << x_error << std::endl;
    std::cout << "y_error: " << y_error << std::endl;

// 粗定位步骤
//    cv::imshow("cropped Image",im_circle);
//    waitKey(0);
//    cv::imshow("cropped Image",im_ring);
//    waitKey(0);


//    for (int r = 0; r < ringTemplate.rows; r++)
//    {
//        const float * ptr = ringTemplate.ptr<float>(r);
//        for (int c = 0; c < ringTemplate.cols; ++c) {
//            cout << ptr[c] << " ";
//        }
//        std::cout << endl;
//    }
//    cv::imshow("circle template",ringTemplate);
//    cv::waitKey(0);


//    //1.中值滤波
//    cv::Mat im_filtered;
//    cv::medianBlur(img,im_filtered,5);
//    std::cout << "2" << std::endl;
//    int center_x,center_y,center_rad,ring_ex_rad,ring_in_rad;

//    cv::Mat g;
//    im_filtered.convertTo(g,CV_8U);

//    //2.二值化
//    cv::threshold(g,g,0,255,cv::THRESH_BINARY | cv::THRESH_OTSU);

//    //3.取反
//    cv::Mat g1 = ~g;
//    //cv::bitwise_not(g,g1);

//    //4.移除小于40000像素
//    cv::Mat labels,stats,centroids;
//    int num_labels = cv::connectedComponentsWithStats(g1,labels,stats,centroids,8,CV_32S);
//    cv::Mat g1_filtered = cv::Mat::zeros(g1.size(),g1.type());
//    for (int i = 1; i < num_labels; ++i)
//    {
//        if (stats.at<int>(i,cv::CC_STAT_AREA) >= 40000)
//        {
//            g1_filtered.setTo(255,labels == i);
//        }
//    }

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count();
    std::cout << "duration time: " << duration << " ms\n";

}
